#!/usr/bin/env python

import roslib
import rospy
import math
import tf
import geometry_msgs.msg
import time
import numpy as np
import csv
from std_msgs.msg import Float64
from scipy.spatial.transform import Rotation as R
from sensor_msgs.msg import LaserScan

# record the distance measured by the laserscan in laser1-4
laser_distance1 = 0
laser_distance2 = 0
laser_distance3 = 0
laser_distance4 = 0
laser_distance5 = 0
laser_distance6 = 0
laser_distance7 = 0
laser_distance8 = 0
laser_distance9 = 0
laser_distance10 = 0

def callback1(msg):
    global laser_distance1
    laser_distance1 = msg.ranges[0]

def callback2(msg):
    global laser_distance2
    laser_distance2 = msg.ranges[0]

def callback3(msg):
    global laser_distance3
    laser_distance3 = msg.ranges[0]

def callback4(msg):
    global laser_distance4
    laser_distance4 = msg.ranges[0]

def callback5(msg):
    global laser_distance5
    laser_distance5 = msg.ranges[0]

def callback6(msg):
    global laser_distance6
    laser_distance6 = msg.ranges[0]

def callback7(msg):
    global laser_distance7
    laser_distance7 = msg.ranges[0]

def callback8(msg):
    global laser_distance8
    laser_distance8 = msg.ranges[0]

def callback9(msg):
    global laser_distance9
    laser_distance9 = msg.ranges[0]

def callback10(msg):
    global laser_distance10
    laser_distance10 = msg.ranges[0]


if __name__ == '__main__':
    # init the node
    rospy.init_node('visual_inspection', anonymous=True)

    # read the tf info form the laserscan to the world
    listener = tf.TransformListener()

    # subscribe to /laser/scan
    rospy.Subscriber("lasersky1/laser/scan", LaserScan, callback1)
    rospy.Subscriber("lasersky2/laser/scan", LaserScan, callback2)
    rospy.Subscriber("lasersky3/laser/scan", LaserScan, callback3)
    rospy.Subscriber("lasersky4/laser/scan", LaserScan, callback4)
    rospy.Subscriber("lasersky5/laser/scan", LaserScan, callback5)
    rospy.Subscriber("lasersky6/laser/scan", LaserScan, callback6)
    rospy.Subscriber("laserground1/laser/scan", LaserScan, callback7)
    rospy.Subscriber("laserground2/laser/scan", LaserScan, callback8)
    rospy.Subscriber("laserground3/laser/scan", LaserScan, callback9)
    rospy.Subscriber("laserground4/laser/scan", LaserScan, callback10)

    # control the lasersky 2 revolute joints
    lasersky1_j5 = rospy.Publisher('lasersky1/joint5_position_controller/command', Float64, queue_size=1)
    lasersky1_j6 = rospy.Publisher('lasersky1/joint6_position_controller/command', Float64, queue_size=1)
    lasersky2_j5 = rospy.Publisher('lasersky2/joint5_position_controller/command', Float64, queue_size=1)
    lasersky2_j6 = rospy.Publisher('lasersky2/joint6_position_controller/command', Float64, queue_size=1)
    lasersky3_j5 = rospy.Publisher('lasersky3/joint5_position_controller/command', Float64, queue_size=1)
    lasersky3_j6 = rospy.Publisher('lasersky3/joint6_position_controller/command', Float64, queue_size=1)
    lasersky4_j5 = rospy.Publisher('lasersky4/joint5_position_controller/command', Float64, queue_size=1)
    lasersky4_j6 = rospy.Publisher('lasersky4/joint6_position_controller/command', Float64, queue_size=1)
    lasersky5_j5 = rospy.Publisher('lasersky5/joint5_position_controller/command', Float64, queue_size=1)
    lasersky5_j6 = rospy.Publisher('lasersky5/joint6_position_controller/command', Float64, queue_size=1)
    lasersky6_j5 = rospy.Publisher('lasersky6/joint5_position_controller/command', Float64, queue_size=1)
    lasersky6_j6 = rospy.Publisher('lasersky6/joint6_position_controller/command', Float64, queue_size=1)

    # control the laserground 2 revolute joints
    laserground1_j3 = rospy.Publisher('laserground1/joint3_position_controller/command', Float64, queue_size=1)
    laserground1_j4 = rospy.Publisher('laserground1/joint4_position_controller/command', Float64, queue_size=1)
    laserground2_j3 = rospy.Publisher('laserground2/joint3_position_controller/command', Float64, queue_size=1)
    laserground2_j4 = rospy.Publisher('laserground2/joint4_position_controller/command', Float64, queue_size=1)
    laserground3_j3 = rospy.Publisher('laserground3/joint3_position_controller/command', Float64, queue_size=1)
    laserground3_j4 = rospy.Publisher('laserground3/joint4_position_controller/command', Float64, queue_size=1)
    laserground4_j3 = rospy.Publisher('laserground4/joint3_position_controller/command', Float64, queue_size=1)
    laserground4_j4 = rospy.Publisher('laserground4/joint4_position_controller/command', Float64, queue_size=1)

    # control the lasersky 3 primistic joints
    lasersky1_j1 = rospy.Publisher('lasersky1/joint1_position_controller/command', Float64, queue_size=1)
    lasersky1_j2 = rospy.Publisher('lasersky1/joint2_position_controller/command', Float64, queue_size=1)
    lasersky1_j3 = rospy.Publisher('lasersky1/joint3_position_controller/command', Float64, queue_size=1)
    lasersky2_j1 = rospy.Publisher('lasersky2/joint1_position_controller/command', Float64, queue_size=1)
    lasersky2_j2 = rospy.Publisher('lasersky2/joint2_position_controller/command', Float64, queue_size=1)
    lasersky2_j3 = rospy.Publisher('lasersky2/joint3_position_controller/command', Float64, queue_size=1)
    lasersky3_j1 = rospy.Publisher('lasersky3/joint1_position_controller/command', Float64, queue_size=1)
    lasersky3_j2 = rospy.Publisher('lasersky3/joint2_position_controller/command', Float64, queue_size=1)
    lasersky3_j3 = rospy.Publisher('lasersky3/joint3_position_controller/command', Float64, queue_size=1)
    lasersky4_j1 = rospy.Publisher('lasersky4/joint1_position_controller/command', Float64, queue_size=1)
    lasersky4_j2 = rospy.Publisher('lasersky4/joint2_position_controller/command', Float64, queue_size=1)
    lasersky4_j3 = rospy.Publisher('lasersky4/joint3_position_controller/command', Float64, queue_size=1)
    lasersky5_j1 = rospy.Publisher('lasersky5/joint1_position_controller/command', Float64, queue_size=1)
    lasersky5_j2 = rospy.Publisher('lasersky5/joint2_position_controller/command', Float64, queue_size=1)
    lasersky5_j3 = rospy.Publisher('lasersky5/joint3_position_controller/command', Float64, queue_size=1)
    lasersky6_j1 = rospy.Publisher('lasersky6/joint1_position_controller/command', Float64, queue_size=1)
    lasersky6_j2 = rospy.Publisher('lasersky6/joint2_position_controller/command', Float64, queue_size=1)
    lasersky6_j3 = rospy.Publisher('lasersky6/joint3_position_controller/command', Float64, queue_size=1)

    # control the laserground 2 primistic joints
    laserground1_j1 = rospy.Publisher('laserground1/joint1_position_controller/command', Float64, queue_size=1)
    laserground1_j2 = rospy.Publisher('laserground1/joint2_position_controller/command', Float64, queue_size=1)
    laserground2_j1 = rospy.Publisher('laserground2/joint1_position_controller/command', Float64, queue_size=1)
    laserground2_j2 = rospy.Publisher('laserground2/joint2_position_controller/command', Float64, queue_size=1)
    laserground3_j1 = rospy.Publisher('laserground3/joint1_position_controller/command', Float64, queue_size=1)
    laserground3_j2 = rospy.Publisher('laserground3/joint2_position_controller/command', Float64, queue_size=1)
    laserground4_j1 = rospy.Publisher('laserground4/joint1_position_controller/command', Float64, queue_size=1)
    laserground4_j2 = rospy.Publisher('laserground4/joint2_position_controller/command', Float64, queue_size=1)

    time.sleep(2)

    # laser to revolute home point
    lasersky1_j5.publish(0.0)
    lasersky1_j6.publish(0.0)
    lasersky2_j5.publish(0.0)
    lasersky2_j6.publish(0.0)
    lasersky3_j5.publish(0.0)
    lasersky3_j6.publish(0.0)
    lasersky4_j5.publish(0.0)
    lasersky4_j6.publish(0.0)
    lasersky5_j5.publish(0.0)
    lasersky5_j6.publish(0.0)
    lasersky6_j5.publish(0.0)
    lasersky6_j6.publish(0.0)

    laserground1_j3.publish(0.0)
    laserground1_j4.publish(0.0)
    laserground2_j3.publish(0.0)
    laserground2_j4.publish(0.0)
    laserground3_j3.publish(0.0)
    laserground3_j4.publish(0.0)
    laserground4_j3.publish(0.0)
    laserground4_j4.publish(0.0)

    # laser to primistic home point
    lasersky1_j1.publish(0.0)
    lasersky1_j2.publish(0.0)
    lasersky1_j3.publish(0.0)
    lasersky2_j1.publish(0.0)
    lasersky2_j2.publish(0.0)
    lasersky2_j3.publish(0.0)
    lasersky3_j1.publish(0.0)
    lasersky3_j2.publish(0.0)
    lasersky3_j3.publish(0.0)
    lasersky4_j1.publish(0.0)
    lasersky4_j2.publish(0.0)
    lasersky4_j3.publish(0.0)
    lasersky5_j1.publish(0.0)
    lasersky5_j2.publish(0.0)
    lasersky5_j3.publish(0.0)
    lasersky6_j1.publish(0.0)
    lasersky6_j2.publish(0.0)
    lasersky6_j3.publish(0.0)

    laserground1_j1.publish(0.0)
    laserground1_j2.publish(0.0)
    laserground2_j1.publish(0.0)
    laserground2_j2.publish(0.0)
    laserground3_j1.publish(0.0)
    laserground3_j2.publish(0.0)
    laserground4_j1.publish(0.0)
    laserground4_j2.publish(0.0)

    time.sleep(3)

    # laser to primistic search pos
    search_pos = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    lasersky3_j1.publish(search_pos[0])
    lasersky3_j2.publish(search_pos[1])
    lasersky3_j3.publish(search_pos[2])
    lasersky4_j1.publish(search_pos[3])
    lasersky4_j2.publish(search_pos[4])
    lasersky4_j3.publish(search_pos[5])
    laserground1_j1.publish(search_pos[6])
    laserground1_j2.publish(search_pos[7])
    laserground3_j1.publish(search_pos[9])
    laserground3_j2.publish(search_pos[10])
    time.sleep(3)

    # all meas point --25
    target_p_all_x =[11.082,10.506,8.069,1.786,-6.825, 1.733,1.733,-2.260,-2.260,4.890, 4.890,-5.126,-5.126, -1 * 1.26 + 0.8, -1.35 * 1.26 + 0.8, -0.65 * 1.26 + 0.8, -1 * 1.26 + 0.8, -1.35 * 1.26 + 0.8, -0.65 * 1.26 + 0.8, -1 * 1.26 + 0.8, -1.35 * 1.26 + 0.8, -0.65 * 1.26 + 0.8, -1 * 1.26 + 0.8, -1.35 * 1.26 + 0.8, -0.65 * 1.26 + 0.8]
    target_p_all_y =[ 2.140, 2.140,0,    0,     0,    -2.200,2.200,-2.200, 2.200,1.380,-1.380,-1.380, 1.380, 0.4 * 1.55,0.2 * 1.55,0.2 * 1.55, -0.4 * 1.55, -0.2 * 1.55,-0.2 * 1.55 ,0.4 * 1.55, 0.2 * 1.55,0.2 * 1.55, -0.4 * 1.55, -0.2 * 1.55,-0.2 * 1.55]
    target_p_all_z =[ 3.500, 3.500,2.150,0.200, 0.200,1.415, 1.415, 0.915, 0.915,2.175, 2.175, 2.175, 2.175, 4.25 * 0.92, 4.25 * 0.92,4.25 * 0.92,4.25 * 0.92,4.25 * 0.92,4.25 * 0.92, 4.25 * 0.92 - 0.25, 4.25 * 0.92 - 0.25, 4.25 * 0.92 - 0.25,4.25 * 0.92 - 0.25,4.25 * 0.92 - 0.25,4.25 * 0.92 - 0.25]

    target_p_all_x.insert(0, 0)
    target_p_all_y.insert(0, -3)
    target_p_all_z.insert(0, 0)

    target_p_all_x.insert(0, 0)
    target_p_all_y.insert(0, -5)
    target_p_all_z.insert(0, 0)

    target_p_all_x.insert(0, 3)
    target_p_all_y.insert(0, -4)
    target_p_all_z.insert(0, 0)

    target_p_all_x.insert(0, -3)
    target_p_all_y.insert(0, -4)
    target_p_all_z.insert(0, 0)

    target_p_all_x.insert(0, -8)
    target_p_all_y.insert(0, -5)
    target_p_all_z.insert(0, 0)

    target_p_all_x.insert(0, 0)
    target_p_all_y.insert(0, 3)
    target_p_all_z.insert(0, 0)

    target_p_all_x.insert(0, 0)
    target_p_all_y.insert(0, 5)
    target_p_all_z.insert(0, 0)

    target_p_all_x.insert(0, 2)
    target_p_all_y.insert(0, 4)
    target_p_all_z.insert(0, 0)

    target_p_all_x.insert(0, -2)
    target_p_all_y.insert(0, 4)
    target_p_all_z.insert(0, 0)

    target_p_all_x.insert(0, 8)
    target_p_all_y.insert(0, 5)
    target_p_all_z.insert(0, 0)

    target_p_all_x.insert(0, 3)
    target_p_all_y.insert(0, -5)
    target_p_all_z.insert(0, 0)

    target_p_all_x.insert(0, 3)
    target_p_all_y.insert(0, -7)
    target_p_all_z.insert(0, 0)

    print('num of target point:', len(target_p_all_x), len(target_p_all_y), len(target_p_all_z))
    target_p_w = np.array([[0],[0],[0]])

    t_number = 0

    laser3_cansee = []
    laser4_cansee = []
    laser7_cansee = []
    laser9_cansee = []

    num_point_canbeseen = 0

    ersOnPoles = np.transpose(np.array([[0.0,0.17,4.0]]))

    while not rospy.is_shutdown():
        #zeroing all lasertracker for the next trial cycle
        try:
            (trans1,rot1) = listener.lookupTransform('/lasersky1/hokuyo_link', '/world', rospy.Time(0))
            (trans2,rot2) = listener.lookupTransform('/lasersky2/hokuyo_link', '/world', rospy.Time(0))
            (trans3,rot3) = listener.lookupTransform('/lasersky3/hokuyo_link', '/world', rospy.Time(0))
            (trans4,rot4) = listener.lookupTransform('/lasersky4/hokuyo_link', '/world', rospy.Time(0))
            (trans5,rot5) = listener.lookupTransform('/lasersky5/hokuyo_link', '/world', rospy.Time(0))
            (trans6,rot6) = listener.lookupTransform('/lasersky6/hokuyo_link', '/world', rospy.Time(0))
            (trans7,rot7) = listener.lookupTransform('/laserground1/hokuyo_link', '/world', rospy.Time(0))
            (trans8,rot8) = listener.lookupTransform('/laserground2/hokuyo_link', '/world', rospy.Time(0))
            (trans9,rot9) = listener.lookupTransform('/laserground3/hokuyo_link', '/world', rospy.Time(0))
            (trans10,rot10) = listener.lookupTransform('/laserground4/hokuyo_link', '/world', rospy.Time(0))

            baseTrans1,baseRot1 = listener.lookupTransform('/world','/lasersky1/link3',rospy.Time(0))
            baseTrans2,baseRot2 = listener.lookupTransform('/world','/lasersky2/link3',rospy.Time(0))
            baseTrans3,baseRot3 = listener.lookupTransform('/world','/lasersky3/link3',rospy.Time(0))
            baseTrans4,baseRot4 = listener.lookupTransform('/world','/lasersky4/link3',rospy.Time(0))
            baseTrans5,baseRot5 = listener.lookupTransform('/world','/lasersky5/link3',rospy.Time(0))
            baseTrans6,baseRot6 = listener.lookupTransform('/world','/lasersky6/link3',rospy.Time(0))

        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            print("unable to get tf data")
            exit()
	
	R_ers = R.from_quat(baseRot1)
	R_ers = R_ers.as_dcm()
	ers_world = np.dot(R_ers, ersOnPoles) + np.transpose([baseTrans1])
	target_p_all_x.insert(0,ers_world[0,0])
	target_p_all_y.insert(0,ers_world[1,0])
	target_p_all_z.insert(0,ers_world[2,0])

	R_ers = R.from_quat(baseRot2)
	R_ers = R_ers.as_dcm()
	ers_world = np.dot(R_ers, ersOnPoles) + np.transpose([baseTrans2])
	target_p_all_x.insert(0,ers_world[0,0])
	target_p_all_y.insert(0,ers_world[1,0])
	target_p_all_z.insert(0,ers_world[2,0])

	R_ers = R.from_quat(baseRot3)
	R_ers = R_ers.as_dcm()
	ers_world = np.dot(R_ers, ersOnPoles) + np.transpose([baseTrans3])
	target_p_all_x.insert(0,ers_world[0,0])
	target_p_all_y.insert(0,ers_world[1,0])
	target_p_all_z.insert(0,ers_world[2,0])

	R_ers = R.from_quat(baseRot4)
	R_ers = R_ers.as_dcm()
	ers_world = np.dot(R_ers, ersOnPoles) + np.transpose([baseTrans4])
	target_p_all_x.insert(0,ers_world[0,0])
	target_p_all_y.insert(0,ers_world[1,0])
	target_p_all_z.insert(0,ers_world[2,0])

	R_ers = R.from_quat(baseRot5)
	R_ers = R_ers.as_dcm()
	ers_world = np.dot(R_ers, ersOnPoles) + np.transpose([baseTrans5])
	target_p_all_x.insert(0,ers_world[0,0])
	target_p_all_y.insert(0,ers_world[1,0])
	target_p_all_z.insert(0,ers_world[2,0])

	R_ers = R.from_quat(baseRot6)
	R_ers = R_ers.as_dcm()
	ers_world = np.dot(R_ers, ersOnPoles) + np.transpose([baseTrans6])
	target_p_all_x.insert(0,ers_world[0,0])
	target_p_all_y.insert(0,ers_world[1,0])
	target_p_all_z.insert(0,ers_world[2,0])

        R1 = R.from_quat(rot1)
        R1 = R1.as_dcm()
        R2 = R.from_quat(rot2)
        R2 = R2.as_dcm()
        R3 = R.from_quat(rot3)
        R3 = R3.as_dcm()
        R4 = R.from_quat(rot4)
        R4 = R4.as_dcm()
        R5 = R.from_quat(rot5)
        R5 = R5.as_dcm()
        R6 = R.from_quat(rot6)
        R6 = R6.as_dcm()
        R7 = R.from_quat(rot7)
        R7 = R7.as_dcm()
        R8 = R.from_quat(rot8)
        R8 = R8.as_dcm()
        R9 = R.from_quat(rot9)
        R9 = R9.as_dcm()
        R10 = R.from_quat(rot10)
        R10 = R10.as_dcm()

        while t_number < len(target_p_all_x) and not rospy.is_shutdown():
            target_p_w = np.array([[target_p_all_x[t_number]],[target_p_all_y[t_number]],[target_p_all_z[t_number]]])
            t_number += 1

            target_p_h1 = np.matmul(R1, target_p_w)
            x1 = trans1[0] + target_p_h1[0][0]
            y1 = trans1[1] + target_p_h1[1][0]
            z1 = trans1[2] + target_p_h1[2][0]
            dist1 = math.sqrt(x1*x1 + y1*y1 + z1*z1)
            lasersky1_j5_pos = math.atan(y1/x1)
            lasersky1_j6_pos = math.atan(z1/math.sqrt(x1*x1 + y1*y1))
            # print(x1, y1, z1, lasersky1_j5_pos, lasersky1_j6_pos)
            #if abs(lasersky1_j5_pos) >= 0.001 or abs(lasersky1_j6_pos) >= 0.001:
            #lasersky1_j5.publish(lasersky1_j5_pos)
            #lasersky1_j6.publish(lasersky1_j6_pos)

            target_p_h2 = np.matmul(R2, target_p_w)
            x2 = trans2[0] + target_p_h2[0][0]
            y2 = trans2[1] + target_p_h2[1][0]
            z2 = trans2[2] + target_p_h2[2][0]
            dist2 = math.sqrt(x2*x2 + y2*y2 + z2*z2)
            lasersky2_j5_pos = math.atan(y2/x2)
            lasersky2_j6_pos = math.atan(z2/math.sqrt(x2*x2 + y2*y2))
            # print(x2, y2, z2, lasersky2_j5_pos, lasersky2_j6_pos)
            #if abs(lasersky2_j5_pos) >= 0.001 or abs(lasersky2_j6_pos) >= 0.001:
            #lasersky2_j5.publish(lasersky2_j5_pos)
            #lasersky2_j6.publish(lasersky2_j6_pos)

            target_p_h3 = np.matmul(R3, target_p_w)
            x3 = trans3[0] + target_p_h3[0][0]
            y3 = trans3[1] + target_p_h3[1][0]
            z3 = trans3[2] + target_p_h3[2][0]
            dist3 = math.sqrt(x3*x3 + y3*y3 + z3*z3)
            lasersky3_j5_pos = math.atan(y3/x3)
            lasersky3_j6_pos = math.atan(z3/math.sqrt(x3*x3 + y3*y3))
            # print(x3, y3, z3, lasersky3_j5_pos, lasersky3_j6_pos)
            if (abs(lasersky3_j5_pos) >= 0.001 or abs(lasersky3_j6_pos) >= 0.001) and x3 >= 0:
                lasersky3_j5.publish(lasersky3_j5_pos)
                lasersky3_j6.publish(lasersky3_j6_pos)
            elif (abs(lasersky3_j5_pos) >= 0.001 or abs(lasersky3_j6_pos) >= 0.001) and x3 < 0:
                lasersky3_j5.publish(lasersky3_j5_pos + math.pi)
                lasersky3_j6.publish(lasersky3_j6_pos)

            target_p_h4 = np.matmul(R4, target_p_w)
            x4 = trans4[0] + target_p_h4[0][0]
            y4 = trans4[1] + target_p_h4[1][0]
            z4 = trans4[2] + target_p_h4[2][0]
            dist4 = math.sqrt(x4*x4 + y4*y4 + z4*z4)
            lasersky4_j5_pos = math.atan(y4/x4)
            lasersky4_j6_pos = math.atan(z4/math.sqrt(x4*x4 + y4*y4))
            # print(x4, y4, z4, lasersky4_j5_pos, lasersky4_j6_pos)
            if (abs(lasersky4_j5_pos) >= 0.001 or abs(lasersky4_j6_pos) >= 0.001) and x4 >= 0:
                lasersky4_j5.publish(lasersky4_j5_pos)
                lasersky4_j6.publish(lasersky4_j6_pos)
            elif (abs(lasersky4_j5_pos) >= 0.001 or abs(lasersky4_j6_pos) >= 0.001) and x4 < 0:
                lasersky4_j5.publish(lasersky4_j5_pos + math.pi)
                lasersky4_j6.publish(lasersky4_j6_pos)


            target_p_h5 = np.matmul(R5, target_p_w)
            x5 = trans5[0] + target_p_h5[0][0]
            y5 = trans5[1] + target_p_h5[1][0]
            z5 = trans5[2] + target_p_h5[2][0]
            dist5 = math.sqrt(x5*x5 + y5*y5 + z5*z5)
            lasersky5_j5_pos = math.atan(y5/x5)
            lasersky5_j6_pos = math.atan(z5/math.sqrt(x5*x5 + y5*y5))
            # print(x5, y5, z5, lasersky5_j5_pos, lasersky5_j6_pos)
            #if abs(lasersky5_j5_pos) >= 0.001 or abs(lasersky5_j6_pos) >= 0.001:
            #lasersky5_j5.publish(lasersky5_j5_pos)
            #lasersky5_j6.publish(lasersky5_j6_pos)

            target_p_h6 = np.matmul(R6, target_p_w)
            x6 = trans6[0] + target_p_h6[0][0]
            y6 = trans6[1] + target_p_h6[1][0]
            z6 = trans6[2] + target_p_h6[2][0]
            dist6 = math.sqrt(x6*x6 + y6*y6 + z6*z6)
            lasersky6_j5_pos = math.atan(y6/x6)
            lasersky6_j6_pos = math.atan(z6/math.sqrt(x6*x6 + y6*y6))
            # print(x6, y6, z6, lasersky6_j5_pos, lasersky6_j6_pos)
            #if abs(lasersky6_j5_pos) >= 0.001 or abs(lasersky6_j6_pos) >= 0.001:
            #lasersky6_j5.publish(lasersky6_j5_pos)
            #lasersky6_j6.publish(lasersky6_j6_pos)

            target_p_h7 = np.matmul(R7, target_p_w)
            x7 = trans7[0] + target_p_h7[0][0]
            y7 = trans7[1] + target_p_h7[1][0]
            z7 = trans7[2] + target_p_h7[2][0]
            dist7 = math.sqrt(x7*x7 + y7*y7 + z7*z7)
            laserground1_j3_pos = math.atan(y7/x7)
            laserground1_j4_pos =  - math.atan(z7/math.sqrt(x7*x7 + y7*y7))
            # print(x7, y7, z7, laserground1_j3_pos, laserground1_j4_pos)
            if (abs(laserground1_j3_pos) >= 0.001 or abs(laserground1_j4_pos) >= 0.001) and x7 >= 0:
                laserground1_j3.publish(laserground1_j3_pos)
                laserground1_j4.publish(laserground1_j4_pos)
            elif (abs(laserground1_j3_pos) >= 0.001 or abs(laserground1_j4_pos) >= 0.001) and x7 < 0:
                laserground1_j3.publish(laserground1_j3_pos + math.pi)
                laserground1_j4.publish(laserground1_j4_pos)

            target_p_h8 = np.matmul(R8, target_p_w)
            x8 = trans8[0] + target_p_h8[0][0]
            y8 = trans8[1] + target_p_h8[1][0]
            z8 = trans8[2] + target_p_h8[2][0]
            dist8 = math.sqrt(x8*x8 + y8*y8 + z8*z8)
            laserground2_j3_pos = math.atan(y8/x8)
            laserground2_j4_pos =  - math.atan(z8/math.sqrt(x8*x8 + y8*y8))
            # print(x8, y8, z8, laserground2_j3_pos, laserground2_j4_pos)
            #if abs(laserground2_j3_pos) >= 0.001 or abs(laserground2_j4_pos) >= 0.001:
            #laserground2_j3.publish(laserground2_j3_pos)
            #laserground2_j4.publish(laserground2_j4_pos)

            target_p_h9 = np.matmul(R9, target_p_w)
            x9 = trans9[0] + target_p_h9[0][0]
            y9 = trans9[1] + target_p_h9[1][0]
            z9 = trans9[2] + target_p_h9[2][0]
            dist9 = math.sqrt(x9*x9 + y9*y9 + z9*z9)
            laserground3_j3_pos = math.atan(y9/x9)
            laserground3_j4_pos =  - math.atan(z9/math.sqrt(x9*x9 + y9*y9))
            # print(x9, y9, z9, laserground3_j3_pos, laserground3_j4_pos)
            if (abs(laserground3_j3_pos) >= 0.001 or abs(laserground3_j4_pos) >= 0.001) and x9 >= 0:
                laserground3_j3.publish(laserground3_j3_pos)
                laserground3_j4.publish(laserground3_j4_pos)
            elif (abs(laserground3_j3_pos) >= 0.001 or abs(laserground3_j4_pos) >= 0.001) and x9 < 0:
                laserground3_j3.publish(laserground3_j3_pos + math.pi)
                laserground3_j4.publish(laserground3_j4_pos)

            target_p_h10 = np.matmul(R10, target_p_w)
            x10 = trans10[0] + target_p_h10[0][0]
            y10 = trans10[1] + target_p_h10[1][0]
            z10 = trans10[2] + target_p_h10[2][0]
            dist10 = math.sqrt(x10*x10 + y10*y10 + z10*z10)
            laserground4_j3_pos = math.atan(y10/x10)
            laserground4_j4_pos =  - math.atan(z10/math.sqrt(x10*x10 + y10*y10))
            # print(x10, y10, z10, laserground4_j3_pos, laserground4_j4_pos)
            #if abs(laserground4_j3_pos) >= 0.001 or abs(laserground4_j4_pos) >= 0.001:
            #laserground4_j3.publish(laserground4_j3_pos)
            #laserground4_j4.publish(laserground4_j4_pos)

            time.sleep(3)

            print('meas_dist:', laser_distance3, laser_distance4, laser_distance7, laser_distance9)
            print('real_dist:', dist3, dist4, dist7, dist9)

            dis_t = 0.2
            print('target_point_world = ', target_p_all_x[t_number - 1], target_p_all_y[t_number - 1], target_p_all_z[t_number - 1])

            n_laser_see = 0

            if (abs(laser_distance3 - dist3) <= dis_t or laser_distance3 >= dist3) and abs(lasersky3_j6_pos) * 180/ math.pi < 45:
                print('lasersky3 can see this point, rot_angle:', lasersky3_j6_pos * 180/math.pi)
                n_laser_see += 1
                laser3_cansee.append(1)
            else:
                laser3_cansee.append(0)

            if (abs(laser_distance4 - dist4) <= dis_t or laser_distance4 >= dist4) and abs(lasersky4_j6_pos) * 180/ math.pi < 45:
                print('lasersky4 can see this point, rot_angle:', lasersky4_j6_pos * 180/math.pi)
                n_laser_see += 1
                laser4_cansee.append(1)
            else:
                laser4_cansee.append(0)

            if (abs(laser_distance7 - dist7) <= dis_t or laser_distance7 >= dist7) and abs(laserground1_j4_pos) * 180/ math.pi < 45:
                print('laserground1 can see this point, rot_angle:', laserground1_j4_pos * 180/math.pi)
                n_laser_see += 1
                laser7_cansee.append(1)
            else:
                laser7_cansee.append(0)

            if (abs(laser_distance9 - dist9) <= dis_t or laser_distance9 >= dist9) and abs(laserground3_j4_pos) * 180/ math.pi < 45:
                print('laserground3 can see this point, rot_angle:', laserground3_j4_pos * 180/math.pi)
                n_laser_see += 1
                laser9_cansee.append(1)
            else:
                laser9_cansee.append(0)

            if n_laser_see >= 1:
                num_point_canbeseen += 1
            print('num_point_canbeseen', num_point_canbeseen)
            print('l3_cansee', laser3_cansee)
            print('l4_cansee', laser4_cansee)
            print('l7_cansee', laser7_cansee)
            print('l9_cansee', laser9_cansee)
            print('')
		
	    time.sleep(5)

        time.sleep(4000)

